#include <physics/physicsworld.h>
#include <assert.h>

cmPhysicsWorld::cmPhysicsWorld()
  : _config (0)
  , _dispatcher (0)
  , _broadphase (0)
  , _solver (0)
  , _world (0)
{
  CE_OBJECT_CONSTR
}

cmPhysicsWorld::~cmPhysicsWorld()
{
  if (_world)
    {
      delete _world;
      _world = 0;
    }
  if (_solver)
    {
      delete _solver;
      _solver = 0;
    }
  if (_broadphase)
    {
      delete _broadphase;
      _broadphase = 0;
    }
  if (_dispatcher)
    {
      delete _dispatcher;
      _dispatcher = 0;
    }
  if (_config)
    {
      delete _config;
      _config = 0;
    }
}


void cmPhysicsWorld::Setup()
{
  _config = new btDefaultCollisionConfiguration ();
  _dispatcher = new btCollisionDispatcher(_config);
  _broadphase = new btDbvtBroadphase ();
  _solver = new btSequentialImpulseConstraintSolver ();
  _world = new btDiscreteDynamicsWorld (_dispatcher, _broadphase, _solver, _config);
}


void cmPhysicsWorld::SetGravity(const ceVector3f &gravity)
{
  assert (_world);
  _world->setGravity(BTVECTOR3(gravity));
}

void cmPhysicsWorld::Step(float timeStep)
{
  assert (_world);
  _world->stepSimulation(timeStep, 1, timeStep);
}


btDiscreteDynamicsWorld* cmPhysicsWorld::GetWorld()
{
  return _world;
}

const btDiscreteDynamicsWorld* cmPhysicsWorld::GetWorld() const
{
  return _world;
}

ceEntityNode* cmPhysicsWorld::GetEntityNode(btCollisionObject *obj)
{
  if (!obj)
    {
      return 0;
    }

  void* up = obj->getUserPointer();
  if (!up)
    {
      return 0;
    }

  return static_cast<ceEntityNode*>(up);
}

btRigidBody* cmPhysicsWorld::AddStaticRigidBody(const ceMatrix4f& matrix, btCollisionShape *shape)
{

  btTransform trans;
  trans.setFromOpenGLMatrix(reinterpret_cast<const btScalar*>(&matrix._00));
  btDefaultMotionState* ms = new btDefaultMotionState(trans);
  btRigidBody::btRigidBodyConstructionInfo info (0.0f,
                                                 ms,
                                                 shape,
                                                 btVector3 (0, 0, 0));
  btRigidBody* body = new btRigidBody (info);
  _world->addRigidBody(body);

  return body;
}

btRigidBody* cmPhysicsWorld::AddStaticRigidBody(ceEntityNode *node, btCollisionShape *shape)
{
  cmEntityMotionState* ms = new cmEntityMotionState (node);

  btRigidBody::btRigidBodyConstructionInfo info (0.0f,
                                                 ms,
                                                 shape,
                                                 btVector3 (0, 0, 0));
  btRigidBody* body = new btRigidBody (info);
  ms->SetRigidBody(body);
  _world->addRigidBody(body);

  body->setUserPointer(node);
  return body;
}

btRigidBody* cmPhysicsWorld::AddKinematicRigidBody(ceEntityNode *node, btCollisionShape *shape)
{
  cmEntityMotionState* ms = new cmEntityMotionState (node);

  btRigidBody::btRigidBodyConstructionInfo info (0.0f,
                                                 ms,
                                                 shape,
                                                 btVector3 (0, 0, 0));
  btRigidBody* body = new btRigidBody (info);
  body->setCollisionFlags(body->getCollisionFlags() |
                          btCollisionObject::CF_KINEMATIC_OBJECT);
  body->setActivationState(DISABLE_DEACTIVATION);
  ms->SetRigidBody(body);
  _world->addRigidBody(body);

  body->setUserPointer(node);
  return body;
}


btRigidBody* cmPhysicsWorld::AddDynamicRigidBody(ceEntityNode *node, btCollisionShape *shape, float mass)
{
  cmEntityMotionState* ms = new cmEntityMotionState (node);

  btVector3 inertia;
  shape->calculateLocalInertia(mass, inertia);
  btRigidBody::btRigidBodyConstructionInfo info (mass,
                                                 ms,
                                                 shape,
                                                 inertia);
  btRigidBody* body = new btRigidBody (info);
  ms->SetRigidBody(body);
  _world->addRigidBody(body);

  body->setUserPointer(node);
  return body;
}


btRigidBody* cmPhysicsWorld::AddDynamicRigidBody(ceEntityNode *node, btCollisionShape *shape, ceVector3f inertia, float mass)
{
  cmEntityMotionState* ms = new cmEntityMotionState (node);

  btVector3 btinertia = BTVECTOR3(inertia);
  btRigidBody::btRigidBodyConstructionInfo info (mass,
                                                 ms,
                                                 shape,
                                                 btinertia);
  btRigidBody* body = new btRigidBody (info);
  ms->SetRigidBody(body);
  _world->addRigidBody(body);

  body->setUserPointer(node);
  return body;
}

void cmPhysicsWorld::RemoveRigidBody(btRigidBody *body)
{
  if (_world && body)
    {
      _world->removeRigidBody(body);
    }
}
